package legoproject;

import lejos.nxt.*;
import java.util.ArrayList;
import lejos.util.Stopwatch;

public class MotorController extends Thread {

    private static final int fullTurnAngle = 1110;
    private static final int fullTurnTime = 2500;
    private boolean stopExecution = false;
    private GarbageCollector collector;
    // Commands for the motors
    private final static int forward = 1,  backward = 2,  stop = 3,  flt = 4;
    private final int maxDriveSpeed = 70;
    private final int minDriveSpeed = 40;

    public MotorController(GarbageCollector collector) {
        this.collector = collector;
    }

    public void run() {
        executeInstructions();
    }

    private void restart() {
        stopExecution = true;
    }

    private void stopAll() {
        GarbageCollector.leftMotorPort.controlMotor(0, stop);
        GarbageCollector.rightMotorPort.controlMotor(0, stop);
    }

    private void floatAll() {
        GarbageCollector.leftMotorPort.controlMotor(0, flt);
        GarbageCollector.rightMotorPort.controlMotor(0, flt);
    }

    public void executeInstructions() {
        ArrayList instructions = collector.instructions;
        for (int i = 0; i < instructions.size(); i++) {
            if (stopExecution) {
                break;
            }
            Instruction inst = (Instruction) instructions.get(i);
            executeInstruction(inst.getType(), inst.getArg1(), inst.getArg2());
        }
        if (stopExecution) {
            stopExecution = false;
            collector.done();
        } else {
            collector.done();
        }
    }

    private int mapPower(int rawPower) {
        //raw power er i [0;50]
        int ajustedPower = rawPower * 2;
        ajustedPower = ajustedPower * ((maxDriveSpeed - minDriveSpeed) / 100);
        ajustedPower = ajustedPower + minDriveSpeed;
        return ajustedPower;
    }

    private void executeInstruction(int type, int arg1, int arg2) {
        GarbageCollector.reporter.executingInstruction(type, arg1, arg2);
        if (type == InstructionType.DRIVE) {
            drive(arg1, arg2);
        } else if (type == InstructionType.ROTATE) {
            rotate(arg1, arg2);
        } else if (type == InstructionType.GRAB) {
            grab();
        } else if (type == InstructionType.RELEASEGRAB) {
            releaseGrab();
        } else if (type == InstructionType.SHUTDOWN) {
            try {
                GarbageCollector.bluetoothManager.sendCommand(CommandType.SHUTDOWN);
            } catch (Exception e) {
            }
            System.exit(0);
        } else if (type == InstructionType.FOLLOWLINE) {
            followLine();
        } else if (type == InstructionType.FINDHOME) {
            int degree = 0;
            try{ degree = GarbageCollector.bluetoothManager.sendCommand(CommandType.FINDHOME);
            } catch(Exception e){}
            //Do some rotate depending on the result
            rotate(degree,0);

        } else if (type == InstructionType.FINDHOMETHENDRIVE) {
            int degree = 0;
            try{ degree = GarbageCollector.bluetoothManager.sendCommand(CommandType.FINDHOME);
            } catch(Exception e){}
            //Do some rotate depending on the result
            rotate(degree,0);
            GarbageCollector.leftMotorPort.controlMotor(maxDriveSpeed, backward);
            GarbageCollector.rightMotorPort.controlMotor(maxDriveSpeed, backward);
            try {
                Thread.sleep(3000);
                floatAll();
            } catch (Exception e) {
                stopAll();
                restart();
            }

        } else {
            System.out.println("CRITCAL");
        }
    }

    /*
     * DRIVE
     * @param arg1 distance
     * @param arg2 balance, 50 is straight
     */
    private void drive(int arg1, int arg2) {
        int leftPower;
        int rightPower;
        int balance = arg2 - 50;
        if(balance == 0){
            leftPower = maxDriveSpeed;
            rightPower = maxDriveSpeed;
        } else if (balance > 0) {
            leftPower = maxDriveSpeed;
            rightPower = mapPower(balance);
        } else if (balance < 0) {
            rightPower = maxDriveSpeed;
            leftPower = mapPower(-balance);
        } else {
            leftPower = maxDriveSpeed;
            rightPower = maxDriveSpeed;
        }
        if(balance == 50){
            leftPower = maxDriveSpeed;
            rightPower = 0;
        }
        if(balance == -50){
            leftPower = 0;
            rightPower = maxDriveSpeed;
        }
        if (arg1 > 0) {
            GarbageCollector.leftMotorPort.controlMotor(leftPower, backward);
            GarbageCollector.rightMotorPort.controlMotor(rightPower, backward);
        } else {
            GarbageCollector.leftMotorPort.controlMotor(leftPower, forward);
            GarbageCollector.rightMotorPort.controlMotor(rightPower, forward);
        }
        try {
            Thread.sleep(Math.abs(arg1));
            floatAll();
        } catch (Exception e) {
            stopAll();
            restart();
        }
    }

    private void rotate(int arg1, int arg2) {
        //arg1 degree fx. 180
        int degree = arg1 * (fullTurnAngle / 360);
        //we wait 50 after the motor stops to make sure it will not be interrupted to soon
        int time = arg1 * (fullTurnTime / 360) + 50;
        GarbageCollector.leftMotor.rotate(degree, true);
        GarbageCollector.rightMotor.rotate(-degree, true);
        try {
            Thread.sleep(Math.abs(time));
            floatAll();
        } catch (Exception e) {
            stopAll();
            restart();
        }
    }

    private void grab() {
        try {
            collector.bluetoothManager.sendCommand(CommandType.GRAB);
            collector.garbageInGrab = true;
            Thread.sleep(1500);
        } catch (Exception e) {
            stopAll();
            restart();
        }
    }


    private void followLine(){
       int timeOut = 3500;
       int corretionTime = 1700;
       Stopwatch timeOnWhite = new Stopwatch();
       int value;
       int leftPower, rightPower;
       while(true){
            if(timeOnWhite.elapsed() > timeOut){
                return;
            }
            boolean leftPressed = GarbageCollector.leftTouchSensor.isPressed();
            boolean rightPressed = GarbageCollector.leftTouchSensor.isPressed();
            if(!leftPressed || !rightPressed){
                if(!leftPressed){
                    rightPower = -60;
                    leftPower = 20;
                }
                else{//(!rightPressed)
                    rightPower = 20;
                    leftPower = -60;
                }
            }
            else{
                value = GarbageCollector.lightSensor.readValue();
                if(value < GarbageCollector.blackThreshold){
                    if(timeOnWhite.elapsed() > corretionTime){
                        rotate(-65, 0);
                        timeOnWhite.reset();
                        leftPower = 55;
                        rightPower = 55;
                    }
                    else{
                        timeOnWhite.reset();
                        leftPower = 75;
                        rightPower = 0;
                    }
                }
                else{
                        leftPower = 0;
                        rightPower = 75;
                }
            }
            GarbageCollector.leftMotorPort.controlMotor(leftPower, backward);
            GarbageCollector.rightMotorPort.controlMotor(rightPower, backward);
            try {
                Thread.sleep(150);
            }catch (Exception e) {
                stopAll();
                restart();
                return;
            }
        }


    }

    private void releaseGrab() {
        try {
            collector.bluetoothManager.sendCommand(CommandType.RELEASEGRAB);
            collector.garbageInGrab = false;
            Thread.sleep(1500);
        } catch (Exception e) {
        }
    }

    private int ajustPower(int power) {
        int res = 0;
        if (power > 50) {
            res = (int) (power * 0.50);
        }
        if (power > 10 && power <= 50) {
            res = 50;
        } else {
            res = 0;
        }
        return res;
    }
}